function [CB_mB_m1, qB_mB_m1, alpha_l, alpha_l1, beta_l, beta_l1] = bodycoordattupdstep(mode, Dalpha_lx, useDCMInAttCalc, beta_m_ext) %#codegen
%BODYCOORDATTUPDSTEP 体坐标系姿态更新解算周期
%
% Input Arguments:
% mode: 标量，运行模式：0为初始化，1为高速解算周期，其它为中速解算周期
% Dalpha_lx: 3*n矩阵，n为圆锥效应补偿算法阶数，各列为高速解算周期内的角增量，第1列为本周期的，第2列为上一周期的，依此类推，单位rad，1模式中调用
% usDCMInAttCalc: 逻辑标量，true表示在姿态解算中使用方向余弦矩阵，false表示使用四元数，默认为true，2模式中调用
% beta_m_ext：3*1向量，外部给出的圆锥效应补偿项，如果不为空或者NaN，则在中速解算周期中使用该值替代内部计算的圆锥效应补偿项，默认为空，2模式中使用
%
% Output Arguments:
% CB_mB_m1: 3*3矩阵，2模式中输出
% qB_mB_m1: 1*4向量，2模式中输出
% alpha_l, alpha_l1: 3*1向量，自本中速周期以来，本高速周期及上一高速周期的陀螺输出角增量的累积值，1模式中输出
% beta_l, beta_l1: 3*1向量，本高速周期及上一高速周期的圆锥效应补偿项，1模式中输出
%
% References:
% [1] 理论文档“捷联惯性导航数值积分算法”， % 青鸟版本0.6 %

persistent p_mid_BCAUS p_cfg_BCAUS_fcnRV2DCM p_cfg_BCAUS_fcnRV2Quat p_cfg_BCAUS_fcnDbeta_l

% persistent变量初始化
% NOTE: 不应依靠在该cell中对persistent变量赋的初值，因该cell仅在persistent变量不存在时执行
if isempty(p_mid_BCAUS)
    p_mid_BCAUS.alpha_l = coder.nullcopy(NaN(3, 1));
    p_mid_BCAUS.beta_l = coder.nullcopy(NaN(3, 1));
end
if isempty(p_cfg_BCAUS_fcnRV2DCM)
    p_cfg_BCAUS_fcnRV2DCM = @rv2dcm_5thod;
end
if isempty(p_cfg_BCAUS_fcnRV2Quat)
    p_cfg_BCAUS_fcnRV2Quat = @rv2quat_5thod;
end
if isempty(p_cfg_BCAUS_fcnDbeta_l)
    p_cfg_BCAUS_fcnDbeta_l = @Dbeta_l_2ndod;
end

if nargin < 4
    beta_m_ext = [];
end
if nargin < 3
    useDCMInAttCalc = true;
end

switch mode
    case 0
        % 初始化
        p_mid_BCAUS.alpha_l = zeros(3, 1);
        p_mid_BCAUS.beta_l = zeros(3, 1);
        CB_mB_m1 = NaN(3);
        qB_mB_m1 = NaN(1, 4);
        alpha_l1 = NaN(3, 1);
        beta_l1 = NaN(3, 1);
    case 1
        % 运行高速解算步骤
        alpha_l1 = p_mid_BCAUS.alpha_l;
        beta_l1 = p_mid_BCAUS.beta_l;
        [p_mid_BCAUS.alpha_l, p_mid_BCAUS.beta_l] = att_hs(alpha_l1, Dalpha_lx, beta_l1, p_cfg_BCAUS_fcnDbeta_l);
        CB_mB_m1 = NaN(3);
        qB_mB_m1 = NaN(1, 4);
    otherwise
        % 运行中速解算步骤
        if isempty(beta_m_ext) || any(isnan(beta_m_ext))
            phi_m = p_mid_BCAUS.alpha_l + p_mid_BCAUS.beta_l; % REF1式8.10
        else
            phi_m = p_mid_BCAUS.alpha_l + beta_m_ext;
        end
        if useDCMInAttCalc
            CB_mB_m1 = p_cfg_BCAUS_fcnRV2DCM(phi_m'); % REF1式8.2
            qB_mB_m1 = NaN(1, 4);
        else
            qB_mB_m1 = p_cfg_BCAUS_fcnRV2Quat(phi_m'); % REF1式8.33
            CB_mB_m1 = NaN(3);
        end
        alpha_l1 = NaN(3, 1);
        beta_l1 = NaN(3, 1);
end
alpha_l = p_mid_BCAUS.alpha_l;
beta_l = p_mid_BCAUS.beta_l;
end